/**
 ******************************************************************************
 * @file: motor.c
 * @brief: 电机驱动
 * @author: Mond
 ******************************************************************************
 * @version: 0.1.0
 * @date: 2021/12/8
 * @lastChange: 2021/12/8
 * @history:
 ******************************************************************************
 */
#include "motor.h"
#include "misc.h"
#include "usbd_cdc_if.h"

/*!
 * 电机控制函数
 * @param speed 电机速度，-100 ~ 100
 * @param hmotor 电机指针
 */
void motor_Control(float speed, Motor *hmotor) {
    if (speed >= 0) {
        HAL_GPIO_WritePin(hmotor->Dir1_GPIOx, hmotor->Dir1_GPIO_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(hmotor->Dir2_GPIOx, hmotor->Dir2_GPIO_Pin, GPIO_PIN_RESET);
    } else {
        speed = -speed;
        HAL_GPIO_WritePin(hmotor->Dir1_GPIOx, hmotor->Dir1_GPIO_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(hmotor->Dir2_GPIOx, hmotor->Dir2_GPIO_Pin, GPIO_PIN_SET);
    }
    TIM_HandleTypeDef *htim = hmotor->Pwm_Htim;
    uint32_t channel = hmotor->Pwm_Channel;
    uint16_t pwm = (uint16_t) ((float) (htim->Instance->ARR) * speed / 100.0);
            __HAL_TIM_SetCompare(htim, channel, pwm);
}

/*!
 * PID 控制器
 * @param hPID PID 结构体
 */
void PID_Compute(PID_TypeDef *hPID) {
    double bias = hPID->target - hPID->current;
    if (bias <= 0.01 && bias >= -0.01) bias = 0;
    if (hPID->result >= 100) {
        if (bias > 0) hPID->integral += 0;
        else hPID->integral += (bias + hPID->last_bias) / 2;
    } else if (hPID->result <= -100) {
        if (bias < 0) hPID->integral += 0;
        else hPID->integral += (bias + hPID->last_bias) / 2;
    } else {
        hPID->integral += (bias + hPID->last_bias) / 2;
    }
    double KP = hPID->P;
    double KI = hPID->I * t;
    double KD = hPID->D / t;
    hPID->result = KP * bias + KI * hPID->integral + KD * (bias - hPID->last_bias);
    hPID->last_bias = bias;
}

/*!
 * PID 控制电机
 * @param speed 电机速度，单位 转每秒
 * @param hmotor 电机结构体指针
 * @param hPID PID 结构体指针
 */
void motor_PID_Control(float speed, Motor *hmotor) {
    hmotor->Velocity_PID->target = speed;
    hmotor->Velocity_PID->current = omega;
    PID_Compute(hmotor->Velocity_PID);
    motor_Control(hmotor->Velocity_PID->result, hmotor);
}


